/*
 * ppmGen.cpp
 *
 *  Created on: 16.04.2013
 *      Author: Jonas
 *	letzes Update:
 Author: Jonas | Zweck: Source-Code aufgeräumt
 */

#include "ppmGen.h"

/*
 * Initialisieren des PPM-Generators
 */
void ppmGenInit() {
	//Datenrichtung einstellen
	DDR_MOTOR_1 |= (1 << PIN_MOTOR_1);
	DDR_MOTOR_2 |= (1 << PIN_MOTOR_2);
	DDR_MOTOR_3 |= (1 << PIN_MOTOR_3);
	DDR_MOTOR_4 |= (1 << PIN_MOTOR_4);

	//Alle Pins ausschalten
	PORT_MOTOR_1 &= ~(1 << PIN_MOTOR_1);
	PORT_MOTOR_2 &= ~(1 << PIN_MOTOR_2);
	PORT_MOTOR_3 &= ~(1 << PIN_MOTOR_3);
	PORT_MOTOR_4 &= ~(1 << PIN_MOTOR_4);

	//0 senden, um Regler zu kalibrieren
	for (uint8_t i = 0; i < 100; i++) {
		//5 wird als 0 festgelegt
		ppmSendSignal(5, 5, 5, 5);
		//kurz warten
		_delay_ms(15);
	}
}

/* 
 * Motorwerte an Regler senden
 */
void ppmSendSignal(int8_t power_motor1, int8_t power_motor2,
		int8_t power_motor3, int8_t power_motor4) {
	//Werte sortieren
	int8_t werte[4] = { power_motor1, power_motor2, power_motor3, power_motor4 };
	//stellt Motorschaltreihenfolge dar:
	uint8_t reihenfolge[4] = { 1, 2, 3, 4 };

	// negative werte abfangen
	for (uint8_t i = 0; i < 3; i++) {
		if (werte[i] < 0) {
			werte[i] = 0;
		}
	}

	// Falls Motorschalter- aus:
	if (empf_wert[4] < 20) {
		werte[0] = 0;
		werte[1] = 0;
		werte[2] = 0;
		werte[3] = 0;
	}
	//Werte in Debug-Array schreiben, zur Auswertung am Rechner (falls Debug erlaubt)
#ifdef DEBUG_ENABLED
	send_buffer[8] = werte[0];
	send_buffer[9] = werte[1];
	send_buffer[10] = werte[2];
	send_buffer[11] = werte[3];
#endif

	//Werte sortieren
	while (1) {
		uint8_t tausch = 0;
		for (uint8_t i = 0; i < 3; i++) {
			if (werte[i] > werte[i + 1]) {
				//vertauschen, wenn links ein niedrigerer Wert steht
				uint8_t temp = werte[i];
				werte[i] = werte[i + 1];
				werte[i + 1] = temp;
				//Reihenfolge angleichen
				temp = reihenfolge[i];
				reihenfolge[i] = reihenfolge[i + 1];
				reihenfolge[i + 1] = temp;
				//Tauschvariable erhöhen
				tausch++;
			}
		}

		//abbrechen, wenn nicht vertauscht werden musste
		//da dann das Array fertig sortiert ist
		if (tausch == 0) {
			break;
		}
	}

	//Werte entsprechend Zeit berechnen
	//Zeit berechnen
	for (uint8_t i = 0; i < 4; i++) {
		// /10 weil schleife in 10 us
		werte[i] = werte[i] * US_PRO_PROZENT;
	}

	//von jedem Wert Vorgängerwerte abziehen:
	//(sonst stimmen Zeitwerte später nicht)
	for (uint8_t i = 0; i < 4; i++) {
		uint8_t counter = i;
		while (counter > 0) {
			counter--;
			werte[i] -= werte[counter];
		}
	}

	//Werte senden
	//Alle Ports einschalten:
	PORT_MOTOR_1 |= (1 << PIN_MOTOR_1);
	PORT_MOTOR_2 |= (1 << PIN_MOTOR_2);
	PORT_MOTOR_3 |= (1 << PIN_MOTOR_3);
	PORT_MOTOR_4 |= (1 << PIN_MOTOR_4);

	// 0% entspricht 1 ms --->>>>> OFFSET
	_delay_us(10 * MINPOWER);

	//Zeitschleifen
	for (uint8_t i = 0; i < 4; i++) {
		//Zeit abwarten
		for (uint8_t k = 0; k < werte[i]; k++) {
			_delay_us(10);
		}
		//Abfrage welcher Motor ausgeschaltet werden muss
		switch (reihenfolge[i]) {
		case 1:
			PORT_MOTOR_1 &= ~(1 << PIN_MOTOR_1);
			break;
		case 2:
			PORT_MOTOR_2 &= ~(1 << PIN_MOTOR_2);
			break;
		case 3:
			PORT_MOTOR_3 &= ~(1 << PIN_MOTOR_3);
			break;
		case 4:
			PORT_MOTOR_4 &= ~(1 << PIN_MOTOR_4);
			break;
		default:
			break;
		}
	}
}

